/**
 * @file screen_driver.cpp
 * @author circleup (circleup@foxmial.com)
 * @brief rviz 显示
 * @version 0.1
 * @date 2020-07-14
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#include "screen_driver.h"

point::point(void)
{
  // 绑定消息发布函数
  point_rviz_pub = point_rviz_handle.advertise<visualization_msgs::Marker>("visualization_marker", 100);

  // 定义点特征
  point_rviz.header.frame_id = "/car";
  point_rviz.header.stamp = ros::Time::now();
  point_rviz.ns = "points";
  point_rviz.action = visualization_msgs::Marker::ADD;
  point_rviz.pose.orientation.w = 1.0;

  point_rviz.id = 0;

  point_rviz.type = visualization_msgs::Marker::POINTS;

  point_rviz.scale.x = 0.2;
  point_rviz.scale.y = 0.2;

  point_rviz.color.r = 1.0f;
  point_rviz.color.a = 1.0;
}

point::~point()
{
}

void point::AddPoint(void)
{
  point_rviz.points.push_back(this_points);
}

void point::publish_to_rviz(void)
{
  point_rviz_pub.publish(point_rviz);
  point_rviz.points.clear();
}

path::path(/* args */)
{
  path_rviz_pub = path_rviz_handle.advertise<nav_msgs::Path>("/plan",1, true);

  path_rviz.header.stamp=ros::Time::now();
  path_rviz.header.frame_id="/car";
  
}

path::~path()
{
}

void path::add_pose(geometry_msgs::PoseStamped this_pose)
{
  path_rviz.poses.push_back(this_pose);
}

void path::publish_to_rviz(void)
{
  path_rviz_pub.publish(path_rviz);
}

obstacle::obstacle(void)
{
  obstacle_rviz_pub = obstacle_rviz_handle.advertise<visualization_msgs::Marker>("visualization_marker", 1);
  obstacle_rviz.header.frame_id = "/car";
  obstacle_rviz.header.stamp = ros::Time::now();
  obstacle_rviz.ns = "obstacle";
  // obstacle_rviz.id = 0;
  obstacle_rviz.type = visualization_msgs::Marker::CUBE;
  obstacle_rviz.lifetime = ros::Duration();
}

obstacle::~obstacle()
{
}

void obstacle::add_obstacle(geometry_msgs::Point this_obstacle)
{
  ROS_INFO("this_obstacle.x is %d, this_obstacle.y is %d", this_obstacle.x, this_obstacle.y);
  obstacle_rviz.points.push_back(this_obstacle);
}

void obstacle::publish_to_rviz(void)
{
  for (uint16_t i = 0; i < obstacle_rviz.points.size(); i++)
  {
    obstacle_rviz.pose.position.x = obstacle_rviz.points[i].x;
    obstacle_rviz.pose.position.y = obstacle_rviz.points[i].y;
    obstacle_rviz.pose.orientation.w = 1.0;

    obstacle_rviz.scale.x = 1.0;
    obstacle_rviz.scale.y = 1.0;
    obstacle_rviz.scale.z = 1.0;

    // obstacle_rviz.color.r = 0.0f;
    obstacle_rviz.color.g = 1.0f;
    // obstacle_rviz.color.b = 0.0f;
    obstacle_rviz.color.a = 1.0;

    obstacle_rviz.header.stamp = ros::Time::now();
    obstacle_rviz.id = i;

    obstacle_rviz_pub.publish(obstacle_rviz);
  }
  obstacle_rviz.points.clear();
  
}

void obstacle::printf_obstacle_pose(void)
{
  for (uint16_t i = 0; i < obstacle_rviz.points.size(); i++)
  {
    ROS_INFO("obstacle_rviz.points[%d].x is %f", i, obstacle_rviz.points[i].x);
    ROS_INFO("obstacle_rviz.points[%d].y is %f", i, obstacle_rviz.points[i].y);
  }
}